
#include <cstring>
#include <cmath>

#include "Vector.h"



std::ostream& operator << (std::ostream &os, Vector3 const &v)
{
    return os << "(" << v(0) << ", " << v(1) << ", " << v(2) << ")\n";
}

Vector3::Vector3( Real *rp)
{
    memcpy(elts_, rp, sizeof(Real)*3);
}

void Vector3::Cross(Vector3 &dest, Vector3 const &v1, Vector3 const &v2)
{
    dest(0) = v1(1) * v2(2) - v2(1) * v1(2);
    dest(1) = v1(2) * v2(0) - v1(0) * v2(2);
    dest(2) = v1(0) * v2(1) - v2(0) * v1(1);
}

Vector3 Vector3::Cross(Vector3 a, Vector3 b)
{
    Vector3 c;
    Cross(c, a, b);
    return c;
}

// damn GDB
Real InnerProduct(Vector3 a, Vector3 b)
{
    return a*b;
}

Real Vector3::Length()const
{
    return sqrt(Length2());
}

void Vector3::Normalize()
{
    *this /= Length();
}

Real EuclidianDistance(Vector3 const& a, Vector3 const &b)
{
    Vector3 c(a);
    c -= b;
    return c.Length();
}

#define VECTOR_INLINE

#ifndef VECTOR_INLINE
#define VECTOR_INLINE inline
#endif

VECTOR_INLINE Vector3::Vector3()
{
    Fill(0.0);
}

VECTOR_INLINE void Vector3::Fill(Real r)
{
    for (int i=0; i<DIM; i++)
        elts_[i] = r;
}

VECTOR_INLINE void Vector3::ScalarMultiply(Vector3 &dest, Real s, Vector3 const& v)
{
    for (int i=0; i<DIM; i++)
        dest(i) = s*v(i);
}

VECTOR_INLINE void Vector3::Add(Vector3& dest, Vector3 const& w, Vector3 const& v)
{
    for (int i=0; i<DIM; i++)
        dest(i) = w(i)+v(i);
}

VECTOR_INLINE void Vector3::Sub(Vector3& dest, Vector3 const& w, Vector3 const& v)
{
    for (int i=0; i<DIM; i++)
        dest(i) = w(i)-v(i);
}

VECTOR_INLINE Real Vector3::IP(Vector3 const& v, Vector3 const& w)
{
    Real r=0.0;
    for (int i=0; i<DIM; i++)
        r += v(i)*w(i);
    return r;
}

VECTOR_INLINE Real operator * (Vector3 const& v1, Vector3 const& v2)
{
    return Vector3::IP(v1, v2);
}

VECTOR_INLINE Real Vector3::StaticLength2(Vector3 const& v)
{
    Real r=0.0;
    for (int i=0; i<DIM; i++)
        r += v(i)*v(i);
    return r;
}

VECTOR_INLINE void Vector3::Set(Real x, Real y, Real z)
{
    elts_[0] = x;
    elts_[1] = y;
    elts_[2] = z;
}

VECTOR_INLINE Vector3::Vector3(Real x, Real y, Real z)
{
    elts_[0] = x;
    elts_[1] = y;
    elts_[2] = z;
}

VECTOR_INLINE Vector3 operator + (Vector3 v1, Vector3 const& v2)
{
    v1 += v2;
    return v1;
}

VECTOR_INLINE Vector3 operator - (Vector3 v1, Vector3 const& v2)
{
    v1 -= v2;
    return v1;
}

VECTOR_INLINE Vector3 operator * (Real r, Vector3 v1)
{
    v1 *= r;
    return v1;
}

VECTOR_INLINE Vector3 operator * (Vector3 v1, Real r)
{
    v1 *= r;
    return v1;
}
